#pragma once

#include "nav_zone.h"
#include <behaviortree_cpp_v3/blackboard.h>
#include <behaviortree_cpp_v3/bt_factory.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>

namespace Sentry_BT
{
  class Blackboard
  {
  private:
    std::shared_ptr<BT::Blackboard> blackboard_;

  public:
    Blackboard()
    {
      blackboard_ = BT::Blackboard::create();

      // 初始化黑板变量
      blackboard_->set("health", 400.0f);
      blackboard_->set("my_outpost_destroyed", false);
      blackboard_->set("enemy_outpost_health", 1500);
      blackboard_->set("target_valid", false);
      blackboard_->set("target_in_range", false);
      blackboard_->set("target_pose", geometry_msgs::Pose());
      blackboard_->set("target_armor_id", -1);
      blackboard_->set("nav_status", static_cast<int>(NavStatus::IDLE));
      blackboard_->set("current_mode", static_cast<int>(NavMode::PATROL));
      blackboard_->set("nav_goal", Point2D{0.0, 0.0});
      blackboard_->set("patrol_index", 0);
      blackboard_->set("patrol_wait_time", 1000);
    }

    template <typename T> void set(const std::string& key, const T& value)
    {
      blackboard_->set(key, value);
    }

    template <typename T> auto get(const std::string& key)
    {
      return blackboard_->get<T>(key);
    }

    std::shared_ptr<BT::Blackboard> getBTBlackboard()
    {
      return blackboard_;
    }

    void updateHealth(float health)
    {
      set("health", health);
    }

    void updateOwnOutpostStatus(bool destroyed)
    {
      set("own_outpost_destroyed", destroyed);
    }

    void updateEnemyOutpostStatus(int health)
    {
      set("enemy_outpost_health", health);
    }

    void updateBonusStatus(bool active)
    {
      set("bonus_active", active);
    }

    void updateTargetInfo(bool valid, const geometry_msgs::Point& position, int armor_id)
    {
      set("target_valid", valid);
      if(valid)
      {
        geometry_msgs::Pose pose;
        pose.position = position;
        set("target_pose", pose);
        set("target_armor_id", armor_id);
      }
    }

    void updateNavStatus(NavStatus status)
    {
      set("nav_status", static_cast<int>(status));
    }
  };
}  // namespace Sentry_BT